#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <float.h>
#include <vector>

#ifdef    WIN32
#include <windows.h>
#include <io.h>
#include <replace/replace.h>
#include <server/error.h>
#else  // WIN32
#include <unistd.h>
#include <sys/ioctl.h>
#include <player/error.h>
#endif // WIN32

#include <usb.h>
#include <ftdi.h>
#include "usbrw.h"
#include "baseicd.h"
#include "psonaricd.h"


// comment this out, if desired

// uncomment to display certain ping ages in s/ms/us, rather than "units"
  #define AGES_IN_REAL_UNITS







///////////////////////////////////////
// Pseudo-ICD:
// Type  Dest  Len   Payload
// 0x23  DSP   0      ---
// 0x23  PC    20     s16 (t1-t0),s16 (t2-t0),...,s16 (t6-t0),u32 count time of ping,u32 count time msg sent; second always >= first.
// 0x2C  DSP   4      s32 starting block, in units of 128 bytes, referenced to detection time.
// 0x2C  PC    129    u08 (0 if invalid addr (junk follows), 1 if valid; if partially invalid, call invalid), 128 bytes of raw data (samples little-endian, if possible)
// 0x2D  DSP   1      u08 :  0 to disable sampling, 1 to enable sampling, and 2 to check the status only.
// 0x2D  PC    1      u08 :  0 if sampling was previously disabled, and 1 if was previously enabled.
// 0x2E  DSP   160    s32 :  biquad
// 0x2E  PC    0      N/A
///////////////////////////////////////
// (8 channels)*(175000 samples/channel/s)*(2 bytes/sample) = 2 800 000 bytes/sec
// so 1 MByte gives ~357ms
// Saving 1 MByte requries 8192x 128-byte blocks.  65536x 128-byte blocks gives 8 MBytes or 2.86s of data.
///////////////////////////////////////



CPSonarICD::CPSonarICD(double dSamplingFreq,double dAgeFreq) :
#if       PSONAR_ICD_VERSION==0
    CBaseICD(DEFLT_RESEND_US,false),
#else  // PSONAR_ICD_VERSION==0
    CBaseICD(200000         ,true ), // 200ms, do use 1 char only.
#endif // PSONAR_ICD_VERSION==0
    m_dSamplingFreq(dSamplingFreq),m_dAgeFreq(dAgeFreq)
{
  assert(dSamplingFreq >= 60000 && dAgeFreq >= 100); // sanity: not reciprocal

  m_bEverGotPing=false;
  for(int i=0; i<NUM_TDIFF; i++) 
    m_aiDiff[i] = 0x7FFF; // never got a ping.
  
  // irrelevant, due to !m_bEverGotPing:
  m_uTimeSonarAbs=m_uTimeSonarNow=m_uOurTimeNow=0; 
}


//////////////////////////////////////////////////////////////////////////////
// GLOBAL HELPERS ////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

static int    inline imin(int    a,int    b) { return (a>b)?b:a; }
static int    inline imax(int    a,int    b) { return (a>b)?a:b; }

#ifdef    _WIN32
static double inline fmin(double a,double b) { return (a>b)?b:a; }
static double inline fmax(double a,double b) { return (a>b)?a:b; }
#endif // _WIN32


//////////////////////////////////////////////////////////////////////////////
// HELPERS/CALLBACKS /////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////


bool CPSonarICD::ValidateOther(unsigned nDataLen, unsigned nMsgType, 
                               unsigned char const *pchPayload, bool bBusy, 
                               bool bNotImpl) {
  assert(pchPayload);
  assert(nDataLen<=255);
  if(nMsgType != 0x23 && !(nMsgType >= 0x2C && nMsgType <= 0x2E) &&
                         !(nMsgType >= 0x30 && nMsgType <= 0x35) ) {
    // Out of our range; let base handle it.
    return CBaseICD::ValidateOther(nDataLen, nMsgType, pchPayload, bBusy, 
                                   bNotImpl);
  }

  if(bBusy) {
    PLAYER_MSG1(1, "ValidateOther: busy in response to type %d", nMsgType);
    return true; // valid.
  }
  if(bNotImpl) {
    PLAYER_MSG1(1, "ValidateOther: type %d not implemented by MCU", nMsgType);
    return true; // valid.
  }

  // I admit to being somewhat inconsistent in choosing what to check here, 
  //   versus what to check in the parsing routine.  All messages check 
  //   length, however.
  switch(nMsgType) {
  case 0x23: // Get Time Delays
    if(nDataLen!=0x14) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x23 (time delays):  %d (wanted 20)", nDataLen);
      return false;
    }
    if((*(unsigned *)(pchPayload+12)) >
       (*(unsigned *)(pchPayload+16)) ) {
      PLAYER_MSG2(0, "CPSonarICD::ValidateOther:  time of ping (0x%08X) is "
                  "newer than time of message (0x%08X)", 
                  *(unsigned *)(pchPayload+12), *(unsigned *)(pchPayload+16));
      return false;
    }
    return true; // otherwise, we have a valid message.  maybe.
  case 0x2C: // Get Raw Memory Block
    if(nDataLen != 129) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x2C (memblock):  %d (wanted 129)", nDataLen);
      return false;
    }
    if(pchPayload[0] != 0 && pchPayload[0] != 1) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  first byte is invalid:  "
                  "0x%02X", (unsigned)(pchPayload[0]));
      return false;
    }
    return true; // otherwise, it's good, even if we don't like it.
  case 0x2D: // Enable/Disable Sampling
    if(nDataLen != 0x01) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x2D (enablesampling):  %d (wanted 1)", nDataLen);
      return false;
    }
    if(pchPayload[0] != 0 && pchPayload[0] != 1) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  first byte is invalid:  "
                  "0x%02X", (unsigned)(pchPayload[0]));
      return false;
    }
    return true;
  case 0x2E: // Filter Coefficients
    if(nDataLen != 0x00) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x2E (filtercoeffs):  %d (wanted 0)", nDataLen);
      return false;
    }
    return true;
  case 0x30: // Identify
    if(nDataLen != 2) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x30 (identify):  %d (wanted 2)", nDataLen);
      return false;
    }
    if(pchPayload[0] != 1 || pchPayload[1] != 0) {
      PLAYER_MSG2(0, "CPSonarICD::ValidateOther:  first byte(s) invalid:  "
                  "0x%02X 0x%02X", (unsigned)(pchPayload[0]),
                  (unsigned)(pchPayload[1]));
      return false;
    }
    return true;
  case 0x31: // Get Time Delays
    if(nDataLen != 13) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x31 (TDoA):  %d (wanted 13)", nDataLen);
      return false;
    }
    if(pchPayload[0] != 0) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  first byte is invalid:  "
                  "0x%02X", (unsigned)(pchPayload[0]));
      return false;
    }
    if((*(unsigned *)(pchPayload+5)) >
       (*(unsigned *)(pchPayload+9)) ) {
      PLAYER_MSG2(0, "CPSonarICD::ValidateOther:  time of ping (0x%08X) is "
                  "newer than time of message (0x%08X)", 
                  *(unsigned *)(pchPayload+5), *(unsigned *)(pchPayload+9));
      return false;
    }
    return true;
  case 0x32: // Set Competition Side
    if(nDataLen != 0) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x32 (setcomp):  %d (wanted 0)", nDataLen);
      return false;
    }
    return true;
  case 0x33: // Set Practice Side
    if(nDataLen != 0) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x33 (setprac):  %d (wanted 0)", nDataLen);
      return false;
    }
    return true;
  case 0x34: // Stop Sampling
    if(nDataLen != 0) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x34 (stopsamp):  %d (wanted 0)", nDataLen);
      return false;
    }
    return true;
  case 0x35: // Sync with Other Board
    if(nDataLen != 5) {
      PLAYER_MSG1(0, "CPSonarICD::ValidateOther:  strange data len on type "
                  "0x35 (syncboards):  %d (wanted 5)", nDataLen);
      return false;
    }
    return true;
  default:
    assert(false); // should've been checked at top of function.
    return false;
  }
}


//////////////////////////////////////////////////////////////////////////////
// ACTUAL ICD MESSAGES ///////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////


// I have to imagine I *always* want to print this.  bPrint might go away.
//   Or perhaps it'll only control whether stale pings are printed.
bool CPSonarICD::FetchTimeDelays(bool &rbStale, bool bPrint) {
  unsigned char pchPayload[MAX_RECEIVE];
  unsigned nInLen;
  rbStale = true; // assume so.

#if       PSONAR_ICD_VERSION==0
  if(!BaseICDHelperNoBSY("TimeDelays", 0x23, 0x00, (const unsigned char *)"",
                         pchPayload, nInLen))
    return false;
  m_usbrw.RefreshTimer();
  assert(nInLen==20); // should've been checked.
#else  // PSONAR_ICD_VERSION==0
  // Idea here is to send U/V until AVR returns msg 0x31, 13-byte payload; see wiki.
  // Failure to respond in time should cause us to return false.
  if(!BaseICDHelperNoBSY("TimeDelays", 0x31, 0x02,
                         (const unsigned char *)"VU", pchPayload, nInLen))
    return false;
  m_usbrw.RefreshTimer();
  assert(nInLen==13); // should've been checked.
#endif // PSONAR_ICD_VERSION==0

  int i,aiDiff[NUM_TDIFF];
#if       PSONAR_ICD_VERSION==0
  for(i=0; i<NUM_TDIFF; i++)
    aiDiff[i] = *(((int16_t *)pchPayload) + i);
  unsigned uTimeSonarAbs = *(unsigned *)(pchPayload+12); // time of ping
  unsigned uTimeSonarNow = *(unsigned *)(pchPayload+16); // time msg sent
#else  // PSONAR_ICD_VERSION==0
  aiDiff[0]   = *(int16_t *)(pchPayload + 1);
  aiDiff[1]   = *(int16_t *)(pchPayload + 3);
  unsigned uTimeSonarAbs = *(unsigned *)(pchPayload+ 5); // time of ping
  unsigned uTimeSonarNow = *(unsigned *)(pchPayload+ 9); // time msg sent
#endif // PSONAR_ICD_VERSION==0
  assert(uTimeSonarAbs<=uTimeSonarNow); // checked in validator.
  // Note that uTimeSonar* is not necessarily measured on same scale as
  //   aiDiffs[]!

  if(!m_bEverGotPing) {
    for(i=0; i<NUM_TDIFF; i++) { assert(m_aiDiff[i]==0x7FFF); }
    assert(!m_uTimeSonarAbs);
  } else {
    for(i=0; i<NUM_TDIFF; i++) { if(m_aiDiff[i]!=0x7FFF) break; }
    assert(i<NUM_TDIFF); // at least one was not equal <==> m_bEverGotPing
  }

  bool bMsgIndicatesPing = false;
  for(i=0; i<NUM_TDIFF; i++) {
    if(aiDiff[i]!=0x7FFF)
      bMsgIndicatesPing = true; // MCU thinks it's ever gotten a ping.
  }

  if(!bMsgIndicatesPing) { // if the MCU never got a valid ping:
    if(m_bEverGotPing) { // but we have...
      PLAYER_MSG0(0, "Warning:  MCU changed its mind about delays; may have "
                  "been power cycled!");
    } else {
      // Save time of message anyway.  This is a frequent occurrence.
      m_uTimeSonarNow = uTimeSonarNow;
      m_uOurTimeNow   = m_usbrw.curtime();
    }
    if(bPrint)
      PrintTimeDelays();
    return true; // we're done here.
  } 
  // From here down, MCU thinks it's seen a valid ping.

  if(!m_bEverGotPing) { // if this is the first ping we have:
    memcpy(m_aiDiff, aiDiff, sizeof(m_aiDiff));
    m_uTimeSonarAbs = uTimeSonarAbs;
    m_uTimeSonarNow = uTimeSonarNow;
    m_uOurTimeNow   = m_usbrw.curtime();
    m_bEverGotPing  = true;
    if(bPrint)
      PrintTimeDelays();
    return true; // quick exit the first time.
  }
  // From here down, PC has seen pings before too.

  if(uTimeSonarAbs<m_uTimeSonarAbs) {
    PLAYER_MSG0(0, "Warning:  MCU decided to backtrack on absolute time of "
                "ping; may have been power cycled!");
    // How should this be handled??? (ToDo: determine)
    return true; // overwrite nothing.
  }
  if(uTimeSonarNow<m_uTimeSonarNow) {
    PLAYER_MSG0(0, "Warning:  MCU decided to backtrack on time of message "
                "sent; may have been power cycled!");
    // How should this be handled??? (ToDo: determine)
    return true; // overwrite nothing.
  }

  if(uTimeSonarAbs==m_uTimeSonarAbs) { // If same ping:
    for(i=0; i<NUM_TDIFF; i++) {
      if(m_aiDiff[i] != aiDiff[i]) {
        PLAYER_MSG0(0, "Got stale ping as response, but MCU changed "
                    "its mind on time differences!  Most likely a bug.");
        // How should this be handled??? (ToDo: determine)
        return true; // overwrite nothing.
      }
    }
    if(bPrint) {
#ifdef    AGES_IN_REAL_UNITS
      PLAYER_MSG2(2, "Stale ping; was %07.3lfs old, is now %07.3lfs old",
                  (m_uTimeSonarNow-m_uTimeSonarAbs)/m_dAgeFreq,
                  (  uTimeSonarNow-  uTimeSonarAbs)/m_dAgeFreq);
#else  // AGES_IN_REAL_UNITS
      PLAYER_MSG2(2, "Stale ping; was %d units old, is now %d units old",
                  m_uTimeSonarNow-m_uTimeSonarAbs,
                    uTimeSonarNow-  uTimeSonarAbs);
#endif // AGES_IN_REAL_UNITS
    }

    // Update time we last spoke.
    m_uTimeSonarNow = uTimeSonarNow;
    m_uOurTimeNow   = m_usbrw.curtime();

    // Should we print a stale ping?  Saying it's stale is probably enough.
    return true;  // done.
  }

  // We got a new ping.
  memcpy(m_aiDiff, aiDiff, sizeof(m_aiDiff));
  m_uTimeSonarAbs = uTimeSonarAbs;
  m_uTimeSonarNow = uTimeSonarNow;
  m_uOurTimeNow   = m_usbrw.curtime();
  rbStale         = false;
  if(bPrint)
    PrintTimeDelays();
  return true;
}

void CPSonarICD::PrintTimeDelays() const {
  assert(m_uTimeSonarAbs<=m_uTimeSonarNow);
  
  // We're really not modifying anything that isn't routinely updated:
  (const_cast<CPSonarICD *>(this))->m_usbrw.RefreshTimer();

  assert(m_usbrw.curtime()>=m_uOurTimeNow);

  int i;
  if(!m_bEverGotPing) {
    for(i=0; i<NUM_TDIFF; i++) {
      assert(m_aiDiff[i]==0x7FFF);
    }
    assert(!m_uTimeSonarAbs);
    PLAYER_MSG2(2, "no pings yet.  MCU said at %d of its units, %.4lfs ago "
                "our time", m_uTimeSonarNow, 
                (m_usbrw.curtime()-m_uOurTimeNow)/10000.);
  } else {
    for(i=0; i<NUM_TDIFF; i++) {
      if(m_aiDiff[i]!=0x7FFF)
        break;
    }
    assert(i<NUM_TDIFF); // at least one was unequal.
#ifdef    AGES_IN_REAL_UNITS
    PLAYER_MSG1(2, "Time from ping to MCU sending msg is %07.3lfms", 
                (m_uTimeSonarNow-m_uTimeSonarAbs)/m_dAgeFreq*1e3);
#else  // AGES_IN_REAL_UNITS
    PLAYER_MSG1(2, "Time from ping to MCU sending msg is %d units", 
                m_uTimeSonarNow-m_uTimeSonarAbs);
#endif // AGES_IN_REAL_UNITS
    PLAYER_MSG1(2, "PC received msg %.4lfs ago.",
           (m_usbrw.curtime()-m_uOurTimeNow)/10000.);


    char szBuf[256];
    int iOffset=0;
    for(i=0; i<NUM_TDIFF; i++) {
      if(m_aiDiff[i] != 0x7FFF) // this one is valid:
        iOffset+=sprintf(szBuf+iOffset, "%05d%c", m_aiDiff[i], i==NUM_TDIFF-1 ?']':',');
      else
        iOffset+=sprintf(szBuf+iOffset, "-----%c", i==NUM_TDIFF-1 ?']':',');
    }
    PLAYER_MSG1(2, "tdiffs: [%s (samples)", szBuf);
    // ToDo: consider printing in units of time.
  }
}

#if       PSONAR_ICD_VERSION==0
bool CPSonarICD::ReadRawData(int iOffsetBlk, unsigned char *achBuf, 
                             bool &bValidBlock) {
  assert(achBuf);
  unsigned char pchPayload[MAX_RECEIVE];
  unsigned nInLen;
  if(!BaseICDHelperNoBSY("RawData", 0x2C, 0x04, 
                         (const unsigned char *)&iOffsetBlk, pchPayload, 
                         nInLen))
    return false;
  assert(nInLen==129); // should've been checked.
  assert(pchPayload[0]==0 || pchPayload[0]==1); // should've been checked.
  if(!pchPayload[0]) { // was not valid address
    bValidBlock = false;
  } else {
    bValidBlock = true ;
    memcpy(achBuf, pchPayload+1, 128);
  }
  
  return true;
}

bool CPSonarICD::SetSampleMode(eSamplMode esm, eSamplMode &esmOld) {
  unsigned char pchPayload[MAX_RECEIVE];
  unsigned nInLen;
  unsigned char chOut;
  switch(esm) {
  case eSamplChk:     chOut = 2; break;
  case eSamplEnable:  chOut = 1; break;
  case eSamplDisable: chOut = 0; break;
  case eSamplUnk:
  default:
    assert(false);
    break;
  }
  if(!BaseICDHelperNoBSY("SampleMode", 0x2D, 0x01, 
                         &chOut, pchPayload, nInLen))
    return false;
  assert(nInLen==1); // should've been checked.
  assert(pchPayload[0]==0 || pchPayload[0]==1); // should've been checked.
  
  esmOld = pchPayload[0] ? eSamplEnable : eSamplDisable;
  return true;
}

bool CPSonarICD::SetFilterCoeffs(int *iCoeffs, unsigned uISF, 
                                 unsigned char uchQfmat, 
                                 unsigned char uchNbiq) {
  assert(iCoeffs);
  assert(uchNbiq >= 1 && uchNbiq <= 12);
  const int nCoeffs = 5*uchNbiq;
  const int nSize1  = nCoeffs*sizeof(*iCoeffs);
  const int nSize2  = 4+1+1;
  assert(nSize1+nSize2 <= 255);
  unsigned char achOut[255];
  unsigned char pchPayload[MAX_RECEIVE];
  unsigned nInLen;
  memcpy(achOut+nSize2, iCoeffs, nSize1);
  achOut[0]               = uchQfmat;
  achOut[1]               = uchNbiq;
  *(unsigned *)(achOut+2) = uISF;

  if(!BaseICDHelperNoBSY("SetFilterCoeffs", 0x2E, nSize1+nSize2, 
                         (const unsigned char *)achOut, pchPayload, 
                         nInLen))
    return false;
  assert(nInLen==0); // should've been checked.
  return true;
}

#else  // PSONAR_ICD_VERSION==0

bool CPSonarICD::IdentifyBoard() {
  unsigned char pchPayload[MAX_RECEIVE];
  unsigned nInLen;
  if(!BaseICDHelperNoBSY("Identify", 0x30, 0x02, (const unsigned char *)"BA",
                         pchPayload, nInLen))
    return false;
  m_usbrw.RefreshTimer();
  assert(nInLen==2); // should've been checked.
  assert(pchPayload[0] == 1); // should've been checked.
  assert(pchPayload[1] == 0); // should've been checked.
  return true;
}
bool CPSonarICD::SetCompetitionSide() {
  unsigned char pchPayload[MAX_RECEIVE];
  unsigned nInLen;
  if(!BaseICDHelperNoBSY("SetComp", 0x32, 0x02, (const unsigned char *)"DG",
                         pchPayload, nInLen))
    return false;
  m_usbrw.RefreshTimer();
  assert(nInLen==0); // should've been checked.
  return true;
}
bool CPSonarICD::SetPracticeSide() {
  unsigned char pchPayload[MAX_RECEIVE];
  unsigned nInLen;
  if(!BaseICDHelperNoBSY("SetPrac", 0x33, 0x02, (const unsigned char *)"HK",
                         pchPayload, nInLen))
    return false;
  m_usbrw.RefreshTimer();
  assert(nInLen==0); // should've been checked.
  return true;
}

#endif // PSONAR_ICD_VERSION==0









//////////////////////////////////////////////////////////////////////////////
// ABSTRACTION FOR DRIVER ////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////





// larger uVerbosity is more verbose.
// returns:
//   0 success
//   ?? (>0 warn, <0 err)
int CPSonarICD::InitialFns(unsigned uVerbosity) {
#if       PSONAR_ICD_VERSION==0
  int ret = CBaseICD::InitialFns(uVerbosity);
  if(ret < 0)
    return ret; // failure.

  bool bWarn = ret ? true : false; // was there a warning?

  if(m_eBoardFn != eBrdFnPasv) {
    PLAYER_MSG0(0, "Not a passive sonar board.  Aborting.");
    return -1; // failed.  don't take someone else's board.
  }

#else  // PSONAR_ICD_VERSION==0

  m_uchMaxPayPc2Mcu = 2; // will only ever send 1 byte, but needs to be 2,
                         //   due to the way the trick works.

  bool bWarn = false;
  SetTimeBIHelp(50000); // 5 seconds (make sure it answers in this time).
  if(!IdentifyBoard()) {
    PLAYER_MSG0(0, "PSonarICD::InitialFns:  did not respond to ident req."
                "  Aborting.");
    return -1; // failed.  don't take someone else's board.
  }
  SetTimeBIHelp(10000); // 1 second  (stuff will be retried).

#endif // PSONAR_ICD_VERSION==0

  return bWarn ? 1 : 0; // return 0 on total success.
}


void CPSonarICD::GetTDoAinSeconds(double *adTDoA, bool *abTDoAValid) const {
  assert(adTDoA && abTDoAValid);
  for(int i=0; i<NUM_TDIFF; i++) {
    if(m_aiDiff[i]==0x7FFF)
      abTDoAValid[i] = false;
    else {
      abTDoAValid[i] = true ;
      adTDoA     [i] = m_aiDiff[i]/m_dSamplingFreq; // in seconds.
    }
  }
}

bool CPSonarICD::GetAbsTimesInSeconds(double &rdTimePing, double &rdTimeMsg) const {
  rdTimeMsg  = m_uTimeSonarNow/m_dAgeFreq;
  if(!m_bEverGotPing)
    return false;

  rdTimePing = m_uTimeSonarAbs/m_dAgeFreq;
  return true;
}
